#include "geometry.h"

#ifndef EPS
#define EPS 1e-12
#endif

#ifndef M_PI
#define M_PI 3.1415926535897932384626433832795;
#endif


void geometry::Resample(std::vector<Eigen::Vector3d>& polyline, 
	double interval
	)
{
	if (polyline.size() < 2)
	{
		return;
	}

	std::vector<Eigen::Vector3d> resample;
	resample.push_back(polyline[0]);

	double residue_len = 0.0;
	for (std::size_t i = 0; i < polyline.size() - 1; i++)
	{
		Eigen::Vector3d p1 = polyline[i];
		Eigen::Vector3d p2 = polyline[i + 1];

		double len = (p1 - p2).norm();
		double total_len = len + residue_len;

		if (total_len < interval)
		{
			residue_len = total_len;
		}
		else
		{
			Eigen::Vector3d dir = (p2 - p1).normalized();
			do 
			{
				Eigen::Vector3d p = p1 + (interval - residue_len) * dir;
				total_len -= interval;
				residue_len = .0;
				resample.push_back(p);
				p1 = p;
			} 
			while (total_len > interval);
			
			residue_len = total_len;
		}
	}
	if (residue_len != .0)
	{
		resample.push_back(polyline[polyline.size() - 1]);
	}
	polyline.swap(resample);
}

double geometry::GetLength(const std::vector<Eigen::Vector3d>& polyline)
{
	double len = .0;
	if (!polyline.empty())
	{
		for (std::size_t  i = 0; i < polyline.size() - 1; i++)
		{
			len += (polyline[i + 1] - polyline[i]).norm();
		}
	}
	return len;
}


double geometry::DistancePtToLine3D(Eigen::Vector3d pt,
	Eigen::Vector3d ptStart,
	Eigen::Vector3d ptEnd
	)
{
	double dDistance = (ptStart - ptEnd).norm();

	if (dDistance < 1e-4)
	{
		return (ptStart - pt).norm();
	}

	Eigen::Vector3d e1 = ptEnd - ptStart;
	Eigen::Vector3d e2 = pt - ptStart;
	Eigen::Vector3d  n = e1.cross(e2);

	return n.norm() / dDistance;
}

double geometry::DistancePtToLine2D(Eigen::Vector2d pt, 
	Eigen::Vector2d ptStart,
	Eigen::Vector2d ptEnd
	)
{
	double dDistance = (ptStart - ptEnd).norm();

	if (dDistance < 1e-4)
	{
		return (pt - ptStart).norm();
	}

	Eigen::Vector2d e1 = ptEnd - ptStart;
	e1 = e1.normalized();

	Eigen::Vector2d e2 = pt - ptStart;
	double prj_dis = e2.dot(e1);
	double dis = (e2[0] * e2[0] + e2[1] * e2[1]) - (prj_dis * prj_dis);
	return std::sqrt(dis);
}

double geometry::DistancePtToLineSegment3D(Eigen::Vector3d pt,
	Eigen::Vector3d ptStart, 
	Eigen::Vector3d ptEnd
	)
{
	double disSegFromTo = (ptStart - ptEnd).norm();

	if (disSegFromTo < 1e-4)
	{
		return (ptStart - pt).norm();
	}

	double distance = -1.0;
	Eigen::Vector3d e1 = pt - ptStart;
	Eigen::Vector3d e2 = ptEnd - ptStart;
	e2 = e2.normalized();
	double dot = e1.dot(e2);

	if ((dot < 0.0) || (dot > disSegFromTo))
	{
		double dDisS = (pt - ptStart).norm();
		double dDisE = (pt - ptEnd).norm();

		if (dDisS < dDisE)
		{
			distance = dDisS;
		}
		else
		{
			distance = dDisE;
		}
	}
	else
	{
		distance = DistancePtToLine3D(pt, ptStart, ptEnd);
	}

	return distance;
}

double geometry::DistancePtToLineSegment2D(Eigen::Vector2d pt,
	Eigen::Vector2d ptStart,
	Eigen::Vector2d ptEnd
	)
{
	double disSegFromTo = (ptStart - ptEnd).norm();

	if (disSegFromTo < 1e-4)
	{
		return (ptStart - pt).norm();
	}

	double distance = -1.0;
	Eigen::Vector2d e1 = pt - ptStart;
	Eigen::Vector2d e2 = ptEnd - ptStart;
	e2 = e2.normalized();
	double dot = e1.dot(e2);

	if ((dot < 0.0) || (dot > disSegFromTo))
	{
		double dDisS = (pt - ptStart).norm();
		double dDisE = (pt - ptEnd).norm();

		if (dDisS < dDisE)
		{
			distance = dDisS;
		}
		else
		{
			distance = dDisE;
		}
	}
	else
	{
		distance = DistancePtToLine2D(pt, ptStart, ptEnd);
	}

	return distance;
}

double geometry::DistancePolylineToPolyline2D(const std::vector<Eigen::Vector3d>& lhs, const std::vector<Eigen::Vector3d>& rhs)
{
	if(lhs.empty() || rhs.empty())
		return -1.0;

	Eigen::Vector3d ptHit;
	int nSegIndex = -1;
	double disL = GetNearestDisPtToPolyLine(lhs[lhs.size() / 2], rhs, ptHit, nSegIndex);
	double disR = GetNearestDisPtToPolyLine(rhs[rhs.size() / 2], lhs, ptHit, nSegIndex);
	double dis = Minimum(disL, disR);
	return dis;
}

bool geometry::IsProject(const Eigen::Vector3d& pnt,
	const Eigen::Vector3d& ptStart,
	const Eigen::Vector3d& ptEnd
	)
{
	double disSegFromTo = (ptStart - ptEnd).norm();
	Eigen::Vector3d e1 = pnt - ptStart;
	Eigen::Vector3d e2 = ptEnd - ptStart;
	e2 = e2.normalized();
	double dot = e1.dot(e2);

	if ((dot < 0.0) || (dot > disSegFromTo))
	{
		return false;
	}
	return true;
}

bool geometry::IsOverlap(const Eigen::Vector3d& s1,
	const Eigen::Vector3d& e1,
	const Eigen::Vector3d& s2,
	const Eigen::Vector3d& e2
	)
{
	if(IsProject(s1, s2, e2))
	{
		return true;
	}
	else if (IsProject(e1, s2, e2))
	{
		return true;
	}
	else if (IsProject(s2, s1, e1))
	{
		return true;
	}
	return false;
}

bool geometry::IsOverlap(const std::vector<Eigen::Vector3d>& line1,
	const std::vector<Eigen::Vector3d>& line2
	)
{
	Eigen::Vector3d s1 = line1[0], e1 = line1[line1.size() - 1];
	Eigen::Vector3d s2 = line2[0], e2 = line2[line2.size() - 1];
	return IsOverlap(s1, e1, s2, e2);
}

bool geometry::GetProjectPointToPolyline(
	const Eigen::Vector3d& pnt,
	const std::vector<Eigen::Vector3d>& lineString,
	Eigen::Vector3d& pntPrj,
	int &nSegIndex)
{
	int nPointCount = lineString.size();
	if (nPointCount < 2)
	{
		return -1.0;
	}

	bool res = false;
	double dMinDistance = -1.0;
	for (int i = 0; i < nPointCount - 1; i++)
	{
		double dTemp = (lineString[i] - lineString[i + 1]).norm();
		if ((dTemp > EPS))
		{
			Eigen::Vector3d e1 = pnt - lineString[i];
			Eigen::Vector3d e2 = lineString[i + 1] - lineString[i];
			e2 = e2.normalized();

			double dot = e1.dot(e2);
			if (dot > 0.0 && dot < dTemp)
			{
				double dDis = DistancePtToLine3D(pnt, lineString[i], lineString[i + 1]);
				if ((dMinDistance < 0) || (dDis < dMinDistance))
				{
					dMinDistance = dDis;
					nSegIndex = i;
					pntPrj = lineString[i] + e2 * dot;
					res = true;
				}
			}
			else if (e1.norm() == 0.0 || dot == 0.0)
			{
				nSegIndex = i;
				pntPrj = lineString[i];
				res = true;
				break;
			}
		}
	}
	return res;
}

double geometry::GetNearestDisPtToPolyLine(const Eigen::Vector3d& pnt,
	const std::vector<Eigen::Vector3d>& lineString,
	Eigen::Vector3d& pntHit,
	int &nSegIndex
	)
{
	int nPointCount = lineString.size();
	if (nPointCount < 2)
	{
		return -1.0;
	}

	double dMinDistance = -1.0;
	for (int i = 0; i < nPointCount - 1; i++)
	{
		double dTemp = (lineString[i] - lineString[i + 1]).norm();
		if ((dTemp > EPS))
		{
			Eigen::Vector3d e1 = pnt - lineString[i];
			Eigen::Vector3d e2 = lineString[i + 1] - lineString[i];
			e2 = e2.normalized();

			double dot = e1.dot(e2);
			if (dot > 0.0 && dot < dTemp)
			{
				double dDis = DistancePtToLine3D(pnt, lineString[i], lineString[i + 1]);
				if ((dMinDistance < 0) || (dDis < dMinDistance))
				{
					dMinDistance = dDis;
					nSegIndex = i;
					pntHit = lineString[i] + e2 * dot;
				}
			}
			else 
			{
				double dDisS = (pnt - lineString[i]).norm();
				double dDisE = (pnt - lineString[i + 1]).norm();

				if (dDisS < dDisE)
				{
					if ((dMinDistance < 0.0) || (dDisS < dMinDistance))
					{
						dMinDistance = dDisS; 
						nSegIndex = i;
						pntHit = lineString[i];
					}
				}
				else
				{
					if ((dMinDistance < 0.0) || (dDisE < dMinDistance))
					{
						dMinDistance = dDisE;
						nSegIndex = i;
						pntHit = lineString[i + 1];
					}
				}
			}
		}
	}
	return dMinDistance;
}

bool geometry::IsPntLeftSideToLine3D(const Eigen::Vector3d& pnt,
	const Eigen::Vector3d& ptStart,
	const Eigen::Vector3d& ptEnd
	)
{
	Eigen::Vector3d cross_res = (ptEnd - ptStart).cross(pnt - ptStart);
	if (cross_res.z() > 0)
	{
		return true;
	}
	return false;
}

bool geometry::IsPntLeftSideToPolyLine3D(const Eigen::Vector3d& pnt,
	const std::vector < Eigen::Vector3d>& lineString
	)
{
	Eigen::Vector3d pntProject;
	int nSegIndex = -1;
	if (GetNearestDisPtToPolyLine(pnt, lineString, pntProject, nSegIndex) != -1.0)
	{
		Eigen::Vector3d pntStart = lineString[nSegIndex];
		Eigen::Vector3d ptEnd = lineString[nSegIndex + 1];
		return IsPntLeftSideToLine3D(pnt, pntStart, ptEnd);
	}
	return false;
}

Eigen::Matrix3d geometry::RotationAroundX(const double& angle)
{
	double cos_angle = double(cos(angle));
	double sin_angle = double(sin(angle));

	Eigen::Matrix3d R;
	R << 1.0, 0.0, 0.0,
		0.0, cos_angle, -sin_angle,
		0.0, sin_angle, cos_angle;

	return R;
}

Eigen::Matrix3d geometry::RotationAroundY(const double& angle)
{
	double cos_angle = double(cos(angle));
	double sin_angle = double(sin(angle));

	Eigen::Matrix3d R;
	R << cos_angle, 0.0, -sin_angle,
		0.0, 1.0, 0.0,
		sin_angle, 0.0, cos_angle;

	return R;
}

Eigen::Matrix3d geometry::RotationAroundZ(const double& angle)
{
	double cos_angle = double(cos(angle));
	double sin_angle = double(sin(angle));

	Eigen::Matrix3d R;
	R << cos_angle, -sin_angle, 0.0,
		sin_angle, cos_angle, 0.0,
		0.0, 0.0, 1.0;
		
	return R;
}

double geometry::D2R(double degree)
{
	return degree / 180.0 * M_PI;
}

double geometry::R2D(double radians)
{
	return radians * 180.0 / M_PI;
}

double geometry::Maximum(double a, double b)
{
	return a > b ? a : b;
}

double geometry::Minimum(double a, double b)
{
	return a < b ? a : b;
}

bool geometry::GetHeadTailDistance(const std::vector<Eigen::Vector3d>& lhs,
	const std::vector<Eigen::Vector3d>& rhs,
	double& head_dis,
	double& tail_dis
	)
{
	int nSegInx = -1;
	Eigen::Vector3d headL = lhs[0], tailL = lhs[lhs.size() - 1];
	Eigen::Vector3d headR = rhs[0], tailR = rhs[rhs.size() - 1];
	Eigen::Vector3d pntPrj;
	head_dis = Minimum(GetNearestDisPtToPolyLine(headL, rhs, pntPrj, nSegInx),
		GetNearestDisPtToPolyLine(headR, lhs, pntPrj, nSegInx));
	tail_dis = Minimum(GetNearestDisPtToPolyLine(tailL, rhs, pntPrj, nSegInx),
		GetNearestDisPtToPolyLine(tailR, lhs, pntPrj, nSegInx));

	if (!geometry::IsPntLeftSideToPolyLine3D(lhs[lhs.size() / 2], rhs))
	{
		head_dis = - head_dis;
		tail_dis = - tail_dis;
	}
	return true;
}

double geometry::GetMinDistance(const std::vector<Eigen::Vector3d>& lhs,
	const std::vector<Eigen::Vector3d>& rhs
	)
{
	int nSegInx = -1;
	Eigen::Vector3d headL = lhs[0], tailL = lhs[lhs.size() - 1];
	Eigen::Vector3d headR = rhs[0], tailR = rhs[rhs.size() - 1];
	Eigen::Vector3d pntPrj;
	double head_dis = Minimum(GetNearestDisPtToPolyLine(headL, rhs, pntPrj, nSegInx),
		GetNearestDisPtToPolyLine(headR, lhs, pntPrj, nSegInx));
	double tail_dis = Minimum(GetNearestDisPtToPolyLine(tailL, rhs, pntPrj, nSegInx),
		GetNearestDisPtToPolyLine(tailR, lhs, pntPrj, nSegInx));
	double min_dis = Minimum(head_dis, tail_dis);

	if (!geometry::IsPntLeftSideToPolyLine3D(headL, rhs))
	{
		min_dis = -min_dis;
	}
	return min_dis;
}

double geometry::GetAngleBetween(const std::vector<Eigen::Vector3d>& lhs,
	const std::vector<Eigen::Vector3d>& rhs)
{
	Eigen::Vector3d dir1 = lhs[lhs.size() - 1] - lhs[0]; dir1.normalize();
	Eigen::Vector3d dir2 = rhs[rhs.size() - 1] - rhs[0]; dir2.normalize();
	return  R2D(std::cos(dir1.dot(dir2)));
}

double geometry::NormalDistribution(double x, double u, double sigma)
{
	return 1 / (std::sqrt(2 * M_PI) * sigma) * std::exp(-0.5 * std::pow((x - u) / sigma, 2));
}